/*
 * File:    Timer2.c
 * Project:	HobbyPilotFirmware (HPF)
 * Author:  Wouter van Verre
 *
 * Created on March 17, 2011, 2:30 PM
 */

#include "Header.h"

// Timer 2 is used for the Output Compare module
// Timer runs at 50Hz
void initTimer2(void)
{
	T2CONbits.TON = 0;		// Turn the Timer2 module off
    T2CONbits.TSIDL = 1;	// Stop when the uC enters idle mode
    T2CONbits.TGATE = 0;	// Disable gated accumulation mode
    T2CONbits.TCKPS = 0b01;	// Set prescaler to 8
    T2CONbits.T32 = 0;		// Disable 32-bit operations
    T2CONbits.TCS = 0;		// Use internal clock as clock source

    PR2 = 36850;          	// ~50Hz (49.98Hz) @ Fcy = 14,74Mhz
}

void __attribute__((__interrupt__, __no_auto_psv__)) _T2Interrupt (void)
{	
	EnterInterrupt();
	
	ServoOut[0].NextValue = ServoOut[0].Setpoint;
	OC1RS = ServoOut[0].NextValue;
	ServoOut[0].CurrentValue = ServoOut[0].NextValue;

	IFS0bits.T2IF = 0;	// Clear the Timer2 interrupt flag
}
